
#include <asm.h>

.global C_SYMBOL(RGB888To565WMMX)

C_SYMBOL(RGB888To565WMMX):

    STMFD   SP!, {R4-R12,LR}
    LDR     R4, [R0]
    LDR     R5, [R1]
    MOV     R12, #0
    LDR     R3, [R0,#0x4]
    LDR     R2, [R0,#0x8]
    MUL     R6, R2, R3
    MOV     R12, R6, LSL #2
//    CMP     R6, R12
//    BLE     locret_11C

    LDR     R0, = 0x00000002
    TMCR    wCGR0, R0
    LDR     R0, = 0x00000003
    TMCR    wCGR1, R0
    LDR     R0, = 0x00000005
    TMCR    wCGR2, R0
    LDR     R0, = 0x0000000B
    TMCR    wCGR3, R0

    WLDRD   wR0, [R4], #8       @pData
    WLDRD   wR1, [R4], #8       @pData

    WLDRD   wR9, [R4], #8       @pData
    WLDRD   wR10, [R4], #8       @pData

loc_DC:

    SUBS     R12, R12, #0x20
    PLD	    [R4,#0x40]

    WUNPCKILW wR2, wR0, wR1
    WUNPCKIHW wR3, wR0, wR1

    WUNPCKILB wR0, wR2, wR3
    WUNPCKIHB wR1, wR2, wR3
    WUNPCKILH wR2, wR0, wR1
    WUNPCKIHH wR3, wR0, wR1

    WUNPCKELUB wR6, wR2     @B
    WUNPCKEHUB wR7, wR2     @G
    WUNPCKELUB wR8, wR3     @R

    WLDRDGT  wR0, [R4], #8

    WSRLHG  wR6, wR6, wCGR1
    WSRLHG  wR7, wR7, wCGR0
    WSRLHG  wR8, wR8, wCGR1

    WLDRDGT  wR1, [R4], #8

    WSLLHG  wR7, wR7, wCGR2
    WSLLHG  wR8, wR8, wCGR3

    WOR     wR6, wR6, wR7
    WOR     wR6, wR6, wR8

    //ADD     R5, R5, #8
    WSTRD   wR6, [R5], #8

    WUNPCKILW wR2, wR9, wR10
    WUNPCKIHW wR3, wR9, wR10

    WUNPCKILB wR9, wR2, wR3
    WUNPCKIHB wR10, wR2, wR3
    WUNPCKILH wR2, wR9, wR10
    WUNPCKIHH wR3, wR9, wR10

    WLDRDGT  wR9, [R4], #8

    WUNPCKELUB wR6, wR2     @B
    WUNPCKEHUB wR7, wR2     @G
    WUNPCKELUB wR8, wR3     @R

    WLDRDGT  wR10, [R4], #8

    WSRLHG  wR6, wR6, wCGR1
    WSRLHG  wR7, wR7, wCGR0
    WSRLHG  wR8, wR8, wCGR1

    WSLLHG  wR7, wR7, wCGR2
    WSLLHG  wR8, wR8, wCGR3

    WOR     wR6, wR6, wR7
    WOR     wR6, wR6, wR8


    //ADD     R5, R5, #8
    WSTRD   wR6, [R5], #8

    BGT     loc_DC

locret_11C:
    LDMFD   SP!, {R4-R12,PC}
